package kinectData.models;

import java.text.DecimalFormat;
import java.util.ArrayList;
import java.util.Date;
import java.util.Hashtable;
import java.util.StringTokenizer;

import javax.vecmath.Point3d;

/** 
 * Parses data from the Kinect, with expectations of the following order:
 * 
 * POSITION, FOOTLEFT, FOOTRIGHT, ANKLELEFT, ANKLERIGHT, KNEELEFT, KNEERIGHT, HIPLEFT, HIPCENTER, 
 * HIPRIGHT, SPINE, HANDLEFT, HANDRIGHT, WRISTLEFT, WRISTRIGHT, ELBOWLEFT, ELBOWRIGHT, SHOULDERLEFT, 
 * SHOULDERCENTER, SHOULDERRIGHT, HEAD
 * 
 * @author Markus L�chtefeld (loechti)
*/
public class Skeleton {
	
	public static enum Limb {POSITION, FOOTLEFT, FOOTRIGHT, ANKLELEFT, ANKLERIGHT, KNEELEFT, 
		KNEERIGHT, HIPLEFT, HIPCENTER, HIPRIGHT, SPINE, HANDLEFT, HANDRIGHT, WRISTLEFT, WRISTRIGHT, 
		ELBOWLEFT, ELBOWRIGHT, SHOULDERLEFT, SHOULDERCENTER, SHOULDERRIGHT, HEAD}
	
	private static final long SAMPLE_RATE = 50;
	private static final int POINT_SAMPLE_SIZE = 5;
	private Hashtable<Limb, Point3d> limbPosition = new Hashtable<Limb, Point3d>();
	private Hashtable<Limb, Point3d> limbVelocity = new Hashtable<Limb, Point3d>();
	private Hashtable<Limb, Point3d> limbAcceleration = new Hashtable<Limb, Point3d>();	
	private Hashtable<Limb, ArrayList<Point3d>> pointHistory = new Hashtable<Limb, ArrayList<Point3d>>();
	private int trackingID = -1;
	private long lastTrackingTime = 0;
	private String profileId = null;
	
	public Skeleton(Skeleton s){
		this.limbPosition = s.limbPosition;
		this.limbVelocity = s.limbVelocity;
		this.limbAcceleration = s.limbAcceleration;
		this.lastTrackingTime = s.lastTrackingTime;
		this.trackingID = s.trackingID;
	}
	
	public Skeleton(String data){
		update(data);
	}
		
	public void update(String data) {
		long currentTime = new Date().getTime();
		if (currentTime - lastTrackingTime >= SAMPLE_RATE) {
			StringTokenizer st = new StringTokenizer(data, "#");
			trackingID = Integer.parseInt(st.nextToken());
			for (Limb limb : Limb.values()) {
				double x = Double.parseDouble(st.nextToken());
				double y = Double.parseDouble(st.nextToken());
				double z = Double.parseDouble(st.nextToken());
				Point3d newPosition = new Point3d(x, y, z);
				newPosition = smooth(limb, newPosition);
				updateMovement(limb, newPosition, currentTime);
				limbPosition.put(limb, newPosition);
			}
			// This must be updated last.
			lastTrackingTime = currentTime;
		}
	}
	
	public void updateMovement(Limb limb, Point3d newPosition, long currentTime) {
		// Check whether skeleton has been idle.
		if ((currentTime - lastTrackingTime) > 300) {
			lastTrackingTime = currentTime;
		}
		double dt = (double)(currentTime - lastTrackingTime)/1000;
		Point3d oldPosition = limbPosition.get(limb);
		Point3d oldVelocity = limbVelocity.get(limb);
		
		// Handle the initial case.
		if (oldPosition == null) {
			limbVelocity.put(limb, new Point3d());
			return;
		}
		
		double vx = (newPosition.x - oldPosition.x)/dt;
		double vy = (newPosition.y - oldPosition.y)/dt;
		double vz = (newPosition.z - oldPosition.z)/dt;
		Point3d newVelocity = new Point3d(vx, vy, vz);
		limbVelocity.put(limb, newVelocity);
		
		double ax = (newVelocity.x - oldVelocity.x)/dt;
		double ay = (newVelocity.y - oldVelocity.y)/dt;
		double az = (newVelocity.z - oldVelocity.z)/dt;
		Point3d newAcceleration = new Point3d(ax, ay, az);
		limbAcceleration.put(limb, newAcceleration);
	}
	
	public Point3d smooth(Limb limb, Point3d newPoint) {
		ArrayList<Point3d> points = pointHistory.get(limb); 
		if (points == null) {
			points = new ArrayList<Point3d>();
			pointHistory.put(limb, points);
		}
		if (points.size() > POINT_SAMPLE_SIZE) {
			points.remove(0);
		}
		points.add(newPoint);
		
		Point3d average = new Point3d();
		for (Point3d point : points) {
			average.x += point.x;
			average.y += point.y;
			average.z += point.z;
		}
		average.x /= points.size();
		average.y /= points.size();
		average.z /= points.size();
				
		return average;
	}
	
	public long getTrackingTime() {
		return lastTrackingTime;
	}

	public int getTrackingID() {
		return trackingID;
	}
	
	public Point3d getPosition(Limb limb) {
		return limbPosition.get(limb);
	}
	
	public Point3d getVelocity(Limb limb) {
		return limbVelocity.get(limb);
	}
	
	public Point3d getAcceleration(Limb limb) {
		return limbAcceleration.get(limb);
	}
	public void setProfileId(String profileId) {
		this.profileId = profileId;
	}

	public String getProfileId() {
		return profileId;
	}
	
	public boolean hasProfileId() {
		return profileId != null;
	}
	
	public String toString() {
		StringBuffer output = new StringBuffer();
		output.append(String.format("%d#%d#", getTrackingTime(), getTrackingID()));
		for (Limb limb : Skeleton.Limb.values()) {
			Point3d a = this.getPosition(limb);
			output.append(a == null ? "NaN#NaN#NaN#" : 
				new DecimalFormat("#.################").format(a.x) + "#" + 
				new DecimalFormat("#.################").format(a.y) + "#" + 
				new DecimalFormat("#.################").format(a.z) + "#");
		}
		return output.toString();
	}
}
